#include "motor.h"
#include <webots/robot.h>
#include <stdio.h>

/*
 * 功能: 初始化Webots电机
 * 参数:
 *   motor_name - 电机名称，必须与Webots中的电机节点名称一致
 *   position - 初始位置设置，单位为弧度(rad)
 *   velocity - 初始速度设置，单位为弧度每秒(rad/s)
 * 返回: 无
 * 说明: 函数内部会创建静态变量保存电机句柄
 */
WbDeviceTag Motor_Init(const char *motor_name, double position, double velocity)
{

    // 创建变量并获取电机句柄
    WbDeviceTag motor_handle;
    motor_handle = wb_robot_get_device(motor_name);

    // 设置电机位置
    wb_motor_set_position(motor_handle, position);

    // 设置电机速度
    wb_motor_set_velocity(motor_handle, velocity);

    return motor_handle;
}
